#include <rtthread.h>
#include <rtdevice.h>
#include <math.h>

#ifdef FINSH_USING_MSH

#include <finsh.h>

struct _pid {
    float SetVoltage;       //定义设定值
    float ActualVoltage;    //定义实际值
    int32_t pwm_pulse;      //定义当前PWM的值
    float err;              //定义偏差值
    float err_next;         //定义上一个偏差值
    float err_last;         //定义最上前的偏差值
    float Kp, Ki, Kd;       //定义比例、积分、微分系数
} pid_data;

// 5 18 16

void PID_init (float kp, float ki, float kd)
{
    pid_data.SetVoltage = 0.0;
    pid_data.ActualVoltage = 0.0;
    pid_data.err = 0.0;
    pid_data.err_last = 0.0;
    pid_data.err_next = 0.0;
    pid_data.Kp = kp;
    pid_data.Ki = ki;
    pid_data.Kd = kd;
}

float PID_realize (float target, float current)
{
    float increment;

    pid_data.SetVoltage = target;
    pid_data.ActualVoltage = current;

    pid_data.err = pid_data.SetVoltage - pid_data.ActualVoltage;
    increment = pid_data.Kp * (pid_data.err - pid_data.err_next) +
                pid_data.Ki * pid_data.err +
                pid_data.Kd * (pid_data.err - 2 * pid_data.err_next + pid_data.err_last);

    pid_data.err_last = pid_data.err_next;
    pid_data.err_next = pid_data.err;

    return increment;
}

#define ADC_DEV_CHANNEL 1
#define PWM_DEV_CHANNEL 0
#define PWM_PERIOD      1999

int pid (int argc, char **argv)
{
    int result = 0;
    struct rt_device_pwm *dev_pwm = RT_NULL;
    struct rt_adc_device *dev_adc = RT_NULL;

    rt_uint32_t adc_value;
    rt_uint32_t voltage;
    rt_int32_t pwm_pulse = 200;
    float ki, kp, kd;
    int count = 0;
    float target = 2000.0;
    float increment;

    dev_pwm = (struct rt_device_pwm *)rt_device_find ("pwm");
    if (!dev_pwm) {
        result = -RT_EIO;
        goto _exit;
    }
    /* 使能设备 */
    result = rt_pwm_enable (dev_pwm, PWM_DEV_CHANNEL);
    if (result != RT_EOK)
        goto _exit;
    /* 设置PWM周期和脉冲宽度调整值 */
    rt_pwm_set (dev_pwm, PWM_DEV_CHANNEL, PWM_PERIOD, pwm_pulse);

    dev_adc = (struct rt_adc_device *)rt_device_find ("adc");
    if (!dev_adc) {
        result = -RT_EIO;
        goto _exit;
    }
    /* 使能设备 */
    result = rt_adc_enable (dev_adc, ADC_DEV_CHANNEL);
    if (result != RT_EOK)
        goto _exit;

    kp = atoi (argv[1]) / 100.0;
    ki = atoi (argv[2]) / 100.0;
    kd = atoi (argv[3]) / 100.0;
    if (argc == 5)
        target = (float)atoi (argv[4]);

    rt_kprintf ("System begin : 0.%02d, 0.%02d, 0.%02d\n",
                (uint16_t) (kp * 100), (uint16_t) (ki * 100), (uint16_t) (kd * 100));
    PID_init (kp, ki, kd);

    while (count < 1000) {
        /* 读取采样值 */
        adc_value = rt_adc_read (dev_adc, ADC_DEV_CHANNEL);
        /* 计算调整值 */
        increment = PID_realize ((float)target, (float)adc_value);
        /* 计算需要调整的PWM的脉宽 */
        if (increment > 0.0) {
            pwm_pulse += (int32_t) fabs (increment / 2);
            if (pwm_pulse > 1999)
                pwm_pulse = 1999;
        } else {
            pwm_pulse -= (int32_t) fabs (increment / 2);
            if (pwm_pulse < 0)
                pwm_pulse = 0;
        }

        /* 设置PWM周期和脉冲宽度调整值 */
        rt_pwm_set (dev_pwm, PWM_DEV_CHANNEL, PWM_PERIOD, pwm_pulse);

        rt_kprintf ("%03d: ", count);
        rt_kprintf ("adc:%04d; ", adc_value);
        /* 转换为对应电压值 */
        voltage = adc_value * 2495 / 4096;
        rt_kprintf ("voltage: %d.%03d; ", voltage / 1000, voltage % 1000);
        rt_kprintf ("pwm: %04d\n", pwm_pulse );
        rt_thread_mdelay (5);
        count++;
    }

    rt_pwm_disable (dev_pwm, PWM_DEV_CHANNEL);

_exit:
    return result;
}
/* 导出到 msh 命令列表中 */
MSH_CMD_EXPORT (pid, pid sample use: pid 20 36 32 2000);
// run: pid 20 36 32

#endif // End of FINSH_USING_MSH